#include "CQuadTreeNode.h"
#include "my_utilities/mischelper.h"

#include <iostream>
using namespace std;

/* ----------------------------------------------- */
/* --------------Start CQuadTreeNode----------- */


CQuadTreeNode::CQuadTreeNode(CQuadTreeNode* parentNode, CRect r, const int mi, unsigned int d) :
    maxItems_(mi),
    depth_(d)
{
    parent_         = parentNode;
    rect_           = r;          //move up later after debug
    isPartitioned_  = false;
    verbose_        = true;       //for debugging
}

CQuadTreeNode::~CQuadTreeNode()
{
    miscH::FreeClear(children_);
}


/* Insertion Methods */
//insert item in this node

void CQuadTreeNode::Insert(CQuadTreePosItem* item)
{
    if (verbose_) cout << "Insert(item) called..." << endl;
    
    if (depth_ < MAX_DEPTH && items_.size() > maxItems_)
    {
        if (!isPartitioned_)
            Partition();
        
        if(InsertInChild(item))
        {
            if (verbose_) //DEBUG/Dave
            {
                for (unsigned int j = 0; j < depth_ + 1; ++j)
                    cout << ">";
                cout << "Insert into child successful!" << endl;
            }
            return;
        }
        else
        {
            if (verbose_)
            {
                for (unsigned int j = 0; j < depth_ + 1; ++j)
                    cout << ">";
                cout << "Insert into child failed...?" << endl;
            }
        }
    }
    
    
    item->SetCurrentNode(this);
    items_.push_back(item);
    if (verbose_)
    {
        for (unsigned int j = 0; j < depth_; ++j)
            cout << ">";
        cout << "Insert into node successful!" << endl;
    }
}


//checks for item bounds and inserts item in a child of the node

bool CQuadTreeNode::InsertInChild(CQuadTreePosItem* item)
{
    if (verbose_) cout << "InsertInChild(item) called..." << endl;
    
    if (!isPartitioned_)
        return false;
        

    if      (children_[TOPLEFT]->ContainsRect(item->GetRect()))
        children_[TOPLEFT]->Insert(item);
    else if (children_[TOPRIGHT]->ContainsRect(item->GetRect()))
        children_[TOPRIGHT]->Insert(item);
    else if (children_[BOTTOMLEFT]->ContainsRect(item->GetRect()))
        children_[BOTTOMLEFT]->Insert(item);
    else if (children_[BOTTOMRIGHT]->ContainsRect(item->GetRect()))
        children_[BOTTOMRIGHT]->Insert(item);
    else
    {
        if (verbose_)
        {
            for (unsigned int j = 0; j < depth_ + 1; ++j)
                cout << ">";
            cout << "Insert into child failed...because it doesn't fit" << endl;
        }
        return false;
    }
        

    return true;
}

//push an item at index i down to one of the children

bool CQuadTreeNode::PushItemDown(int i)
{
    if (verbose_) cout << "PushItemDown(i) called..." << endl;
    
    if (InsertInChild(items_[i]))
    {
        RemoveItem(i);
        return true;
    }

    return false;
}

//push an item at index i up to parent

void CQuadTreeNode::PushItemUp(int i)
{
    if (verbose_) cout << "PushItemUp(i) called..." << endl;
    
    // if (parent_ == 0)
    //     return;

    if (depth_ > 0)
    {
        if (parent_->ContainsRect(items_[i]->GetRect()))
        {
            parent_->Insert(items_[i]);
            RemoveItem(i);
        }
        else 
            cout << "PushItemUp(i) failed..." << endl;
    }
}

void CQuadTreeNode::PushAllItemsUp()
{
    if (verbose_) cout << "PushAllItemsUp() called..." << endl;
    unsigned int j;
    while (j < items_.size())
    {
        PushItemUp(j);
    }
}

//repartitions node to four quads

bool CQuadTreeNode::Partition()
{
    if (verbose_) cout << "Partition() called..." << endl;
    
    if (depth_ >= MAX_DEPTH)
         return false;

    Vec2f midPoint = rect_.GetCenter();

    //top left
    children_.push_back(new CQuadTreeNode(this, CRect(rect_.GetTopLeft(), midPoint), maxItems_, depth_ + 1));

    //top right
    children_.push_back(new CQuadTreeNode(this, CRect(Vec2f(midPoint.x, rect_.GetTop()), Vec2f(rect_.GetRight(), midPoint.y)), maxItems_, depth_ + 1));

    //bottom left
    children_.push_back(new CQuadTreeNode(this, CRect(Vec2f(rect_.GetLeft(), midPoint.y), Vec2f(midPoint.x, rect_.GetBottom())), maxItems_, depth_ + 1));

    //bottom right
    children_.push_back(new CQuadTreeNode(this, CRect(midPoint, rect_.GetBottomRight()), maxItems_, depth_ + 1));
    
    unsigned int i = 0;
    
    isPartitioned_ = true;
    
    while (i < items_.size())
    {
        if (!PushItemDown(i))
            ++i;
        else
            cout << "PushItemDown successful! items_ now size " << items_.size() << endl;
    }

    return true;
}


/* Query Methods */
//gets list of items containing this point

void CQuadTreeNode::QueryItems(const Vec2f& point, vector<CQuadTreePosItem* >& itemsFound)
{
    if (rect_.Contains(point))
    {
        unsigned int i;
        for (i = 0; i < items_.size(); ++i)
        {
            if (items_[i]->GetRect().Contains(point))
                itemsFound.push_back(items_[i]);
        }

        if (isPartitioned_)
        {
            for (i = 0; i < 4; ++i)
                children_[i]->QueryItems(point, itemsFound);
        }
    }
}

//gets list of items intersecting rect

void CQuadTreeNode::QueryItems(const CRect& r,  vector<CQuadTreePosItem* >& itemsFound)
{
    CRect tr = r;
    if (rect_.Intersects(r))
    {
        unsigned int i;

        for (i = 0; i < items_.size(); ++i)
        {
            if (items_[i]->GetRect().Intersects(r))
                itemsFound.push_back(items_[i]);
        }

        if (isPartitioned_)
        {
            for (i = 0; i < 4; ++i)
                children_[i]->QueryItems(r, itemsFound);
        }
    }
}

//DEBUG/Dave - checks if a node contains a point

bool CQuadTreeNode::DebugContainsPt(const Vec2f& point)
{
    return rect_.Contains(point);
}

bool CQuadTreeNode::DebugDeepestNodeContainingPt(const Vec2f& point)
{
    return (rect_.Contains(point) && !(isPartitioned_));
}

//DEBUG/Dave - get number of items in a node (target node is "deepest" leaf containing point)

void CQuadTreeNode::DebugQueryItems(const Vec2f& point, vector<CQuadTreePosItem* >& itemsFound, Vec2f& identifier, unsigned int& debugDepth)
{
    unsigned int i;
    if (isPartitioned_)
    {
        for (i = 0; i < 4; ++i)
        {
            if (children_[i]->DebugContainsPt(point))   //search for "deepest" node with point
            {
                children_[i]->DebugQueryItems(point, itemsFound, identifier, debugDepth);
                return;
            }
        }
    }
    else //not partitioned - target/base case
    {
        GetAllItems(itemsFound);                //give # of items
        identifier = rect_.GetCenter();         //give identifier
        debugDepth = depth_;                    //give depth
        return;
    }
}

//DEBUG/Dave - get number of items in parent node (target node is second-to-lowest leaf containing point)

void CQuadTreeNode::DebugQueryItemsInParent(const Vec2f& point, vector <CQuadTreePosItem* >& itemsFound, Vec2f& identifier, unsigned int& debugDepth)
{
    unsigned int i;
    
    for (i = 0; i < 4; ++i)
    {
        if (children_[i]->DebugDeepestNodeContainingPt(point))  //found our target!
        {
            GetAllItems(itemsFound);                //give items
            identifier = rect_.GetCenter();         //give identifier
            debugDepth = depth_;                    //give depth
            return;
        }
        else if (children_[i]->DebugContainsPt(point))          //keep searching...
        {
            children_[i]->DebugQueryItemsInParent(point, itemsFound, identifier, debugDepth);
        }
    }
}

//DEBUG/Dave - prints # of items in current node and all children nodes

void CQuadTreeNode::DebugPrintItems()
{
    unsigned int i;
    
    for (i = 0; i < depth_; ++i)
        cout << ">"; //tabs
        
    cout << "L" << depth_ << " Node has "<< items_.size() << " items" << endl;
    
    if (isPartitioned_)
    {
        for (i = 0; i < 4; ++i)
            children_[i]->DebugPrintItems();
    }
}

//gets list of all items in this node

void CQuadTreeNode::GetAllItems(vector<CQuadTreePosItem* >& itemsFound)
{
    unsigned int i;

    for (i = 0; i < items_.size(); ++i)
        itemsFound.push_back(items_[i]);

    if (isPartitioned_)
    {
        for (i = 0; i < 4; ++i)
            children_[i]->GetAllItems(itemsFound);
    }

}

//Finds node containing this item

CQuadTreeNode* CQuadTreeNode::FindItemNode(CQuadTreePosItem* item)
{
    if (miscH::VectorFind(items_, item))
        return this;
    else if (isPartitioned_)
    {
        CQuadTreeNode* n = 0;

        for (unsigned int i = 0; i < 4; ++i)
        {
            if(children_[i]->ContainsRect(item->GetRect()) && !n)
                n = children_[i]->FindItemNode(item);
        }

        return n;
    }
    else
        return 0;
}

//removes item from this node
//
//void CQuadTreeNode::RemoveItem(CQuadTreePosItem* item)
//{
//    vector< CQuadTreePosItem >::iterator it;
//
//    it = find(items_.begin(), items_.end(), item);
//    if (it != items_.end())
//        items_.erase(it);
//    else
//    {
//        assert(false);
//    }
//
//    if (items_.size() == 0)
//        parent_->FlushChildren();
//
//}

//removes item from this node at index

void CQuadTreeNode::RemoveItem(unsigned int i)
{
    if (verbose_) cout << "RemoveItem called" << endl;
    
    if (i < items_.size())
    {
        items_.erase(items_.begin() + i);
    }
    
    if (items_.empty())
    {
        if (depth_ > 0)
            parent_->FlushChildren();
    }
}


/* Misc */
//updates item movement stuff

void CQuadTreeNode::ItemMove(CQuadTreePosItem* item)
{
    vector<CQuadTreePosItem*>::iterator it;

    it = find(items_.begin(), items_.end(), item);
    if (it == items_.end())
    {
        assert(false);
        return;
    }


    int i = distance(items_.begin(), it);


    CRect newR = item->GetRect();

    if (!ContainsRect(newR))
    {
        if (!PushItemDown(i)) //debug this
            PushItemUp(i);
//        CQuadTreeNode* p;
//        if (parent_ == 0)
//        {
//            p = this;
//        }
//        else
//            p = FindNewNode(newR);
//
//        p->Insert(item);
//        RemoveItem(i);
    }
//    else
//    {
//        PushItemDown(i);
//    }

}


//tests if this node contains the rect

bool CQuadTreeNode::ContainsRect(const CRect& r)
{
    return (rect_.GetLeft()   <= r.GetLeft()    &&
            rect_.GetTop()    <= r.GetTop()     &&
            rect_.GetRight()  >= r.GetRight()   &&
            rect_.GetBottom() >= r.GetBottom()  );
}

bool CQuadTreeNode::IsEmpty()
{
    cout << "IsEmpty() called..." << endl;
    
    if (!items_.empty())
        return false;

    for (unsigned int i = 0; i < children_.size(); ++i)
    {
        if (!children_[i]->IsEmpty())
            return false;
    }

    return true;
}

//sees if children are empty, if so deletes them
void CQuadTreeNode::FlushChildren()
{   
    cout << "FlushChildren called..." << endl;
    unsigned int i, j;
    
    vector<CQuadTreePosItem*> items;
    GetAllItems(items);
    if (items.size() <= maxItems_)
    {
        for (i = 0; i < children_.size(); ++i)
        {
            if (!children_[i]->IsEmpty())
            {
                children_[i]->PushAllItemsUp();
            }
        }
    }
    else
    {
        return;
    }
    
//    for (i = 0; i < children_.size(); ++i)
//    {
//        if (!children_[i]->IsEmpty())
//        {
//            cout << ">FlushChildren exited, at least one child not empty." << endl;
//            return;
//        }
//    }

    DestroyChildren();

}

//sees if children is empty, if so destroys them - DEBUG/Dave - never called...
void CQuadTreeNode::DestroyChildren()
{
    cout << "DestroyChildren() called..." << endl;
    for (unsigned int i = 0; i < children_.size(); ++i)
    {
        children_[i]->DestroyChildren(); //it's genocide!!
        cout << "child " << i << " destroyed..." << endl;
    }

    miscH::FreeClear(children_);
    children_.clear();
    isPartitioned_ = false;
}

//searches upward to find a new leaf for a position item that has moved
CQuadTreeNode* CQuadTreeNode::FindNewNode(const CRect& r)
{
    //first find children
    for (unsigned int i = 0; i < children_.size(); ++i)
    {
        if (children_[i]->ContainsRect(r));
            return children_[i];
    }

    if (ContainsRect(r))
        return this;

    if (depth_> 0)
        return parent_->FindNewNode(r);     //if no luck, search up
    else //root node
        return this;                        //exhausted
}


bool CQuadTreeNode::operator==(const CQuadTreeNode& n) const
{
    return (depth_ == n.depth_ && n.parent_ == parent_ && rect_ == n.rect_);
}


/* ----- */

//getters

CRect& CQuadTreeNode::GetRect()
{
    return rect_;
}

//setters
